﻿using UnityEngine;
using System.Collections;
using System.Collections.Generic;
using Kinect = Windows.Kinect;
using Windows.Kinect;
using Joint = Windows.Kinect.Joint;

public class BodySourceView : MonoBehaviour
{
	public Material BoneMaterial;
	public GameObject BodySourceManager;
	private Dictionary<ulong, GameObject> _Bodies = new Dictionary<ulong, GameObject>();
	private BodySourceManager _BodyManager;
	public GameObject _jointObj;
	public Transform initialTrans;

	private Dictionary<Kinect.JointType, Kinect.JointType> _BoneMap = new Dictionary<Kinect.JointType, Kinect.JointType>();
	List<JointType> _joints = new List<JointType> {
		JointType.Head,
	};
	void Update()
	{
		if (BodySourceManager == null)
		{
			return;
		}

		_BodyManager = BodySourceManager.GetComponent<BodySourceManager>();
		if (_BodyManager == null)
		{
			return;
		}

		Body[] data = _BodyManager.GetData();
		if (data == null)
		{
			return;
		}

		List<ulong> trackedIds = new List<ulong>();
		foreach (var body in data)
		{
			if (body == null)
			{
				continue;
			}

			if (body.IsTracked)
			{
				trackedIds.Add(body.TrackingId);
			}
		}

		List<ulong> knownIds = new List<ulong>(_Bodies.Keys);

		// First delete untracked bodies
		foreach (ulong trackingId in knownIds)
		{
			if (!trackedIds.Contains(trackingId))
			{
				Destroy(_Bodies[trackingId]);
				_Bodies.Remove(trackingId);
			}
		}

		foreach (var body in data)
		{
			if (body == null)
			{
				continue;
			}

			if (body.IsTracked)
			{
				if (!_Bodies.ContainsKey(body.TrackingId))
				{
					_Bodies[body.TrackingId] = CreateBodyObject(body.TrackingId);
				}

				RefreshBodyObject(body, _Bodies[body.TrackingId]);
			}
		}
	}

	private GameObject CreateBodyObject(ulong id)
	{
		GameObject body = new GameObject("Body:" + id);

		foreach (Kinect.JointType jt in _joints)
		{
			GameObject jointObj = Instantiate(_jointObj);
			jointObj.name = jt.ToString();
			jointObj.transform.parent = body.transform;
		}
		body.transform.position += initialTrans.position;
		return body;
	}

	private void RefreshBodyObject(Kinect.Body body, GameObject bodyObject)
	{
		foreach (Kinect.JointType jt in _joints)
		{
			Kinect.Joint sourceJoint = body.Joints[jt];
			Vector3 targetPos = GetVector3FromJoint(sourceJoint);
			Transform jointObj = bodyObject.transform.Find(jt.ToString());
			jointObj.position = targetPos;
		}
	}

	private static Vector3 GetVector3FromJoint(Kinect.Joint joint)
	{
		return new Vector3(joint.Position.X * -10, -2.29f, joint.Position.Z * -10+35) ;
	}
}
